#include <ServoPWMF.h>
#include <WString.h>  
#include <avr/pgmspace.h>
#include <inttypes.h>

//Bender variable
unsigned long lastCommandTime=0;
unsigned long lastLRSampleTime=0;
//Time = OriginalTime * 61.03515625 / 976.5625
unsigned int benderSampleRate = 150 * 61.03515625 / 976.5625; //150ms cmd sampling rate;
int RTV;
int LTV;
int RPin = 2;
int LPin = 0;
String LRCmd = String(20);       // allocate a new String
boolean lastLR = false;
int RbenderVal;
int LbenderVal;
int RBenderThres = 30;
int LBenderThres = 20;
// EMG variable 
boolean emgAvailable = false;
int emgPin = 3;
int emgThres = 100;
int emgData[200];
unsigned int emgSampleRate = 50; // in microseconds using delayMicroseconds, doesn't get affected by timer0

//BEEPER
int BPin = 2;
boolean beepOn = true;

//Action Array
PROGMEM  prog_uint8_t relax[]  = { 0, 0, 0, 0, 0};
PROGMEM  prog_uint8_t pinch[]  = { 180, 180, 0, 0, 0};
PROGMEM  prog_uint8_t grab[]  = { 180, 180, 180, 180, 180};
PROGMEM  prog_uint8_t point[]  =  { 180, 0, 180, 180, 180};
PROGMEM  prog_uint8_t count2[]  =  { 180, 0, 0, 180, 180};
PROGMEM  prog_uint8_t count3[]  =  { 180, 0, 0, 0, 180};
PROGMEM  prog_uint8_t count4[]  =  { 180, 0, 0, 0, 0};




ServoPWMF s;

void setup() {
  Serial.begin(9600);
  s.setupTimer();
  pinMode(BPin, OUTPUT);      // sets the digital pin as output
  int RDTV[100];
  int LDTV[100];
  for(int i=0; i< 100; i++)
  {
      RDTV[i] = analogRead(RPin);
      LDTV[i] = analogRead(LPin);
      delayMicroseconds(10000);
  }
  RTV = avg(RDTV) + RBenderThres;
  LTV = avg(LDTV) + LBenderThres;
  
  Serial.print("RDTV:");
  Serial.print(avg(RDTV));
  Serial.print("  LDTV:");
  Serial.println(avg(LDTV));
 free(RDTV);
 free(LDTV);
 int count=0;
 while(false)
 {
   sampleEMG();
   if(emgAvailable)
   {
     beep(300,1700);
     count++;
     resetEmg();
   }
   if(count >=3)
     break;
 }
  
  
}

int avg(int data[])
{
  long sum = 0;
  for(int i=0; i < sizeof(data)/sizeof(int); i++)
  {
     sum = sum + data[i]; 
  }
  return sum/ ((float)sizeof(data)/sizeof(int));
}

//beep for length milli seconds, length > 16
void beep(int length, int silent)
{
  if(beepOn)
  {
   length = length * 61.03515625 / 976.5625;
   silent = silent * 61.03515625 / 976.5625;
    digitalWrite(BPin, HIGH);
    delay(length);
    digitalWrite(BPin, LOW);
    delay(silent);
  }
}



void loop() 
{
  sampleBender();
  sampleEMG();
  performAction();
}

void sampleEMG()
{
  int emgVal = analogRead(emgPin);
  //detect new signal
  if( emgVal > emgThres)
  {
    for(int i=0; i< sizeof(emgData)/sizeof(int); i++)
    {
      emgData[i] = analogRead(emgPin);
      delayMicroseconds(emgSampleRate); //delay 1 millisecond -- emg sampling period 
    } 
    emgAvailable = true;
  }
}


//does not perform action if cmdAvailable == false
void performAction()
{
  if( emgAvailable)
  {
    //GRAB
    if(!strcmp(LRCmd, "R"))
    {
      resetBender();
      resetEmg();
      while(true)
      {
        sampleEMG();
        if( emgAvailable )
        {
           s.incrementT(180, 3);
           s.incrementF1(220, 3);
           s.incrementF2(180, 3);
           s.incrementF3(180, 3);
           s.incrementF4(180, 3);
           resetEmg(); 
        }
        sampleBender();
        if( LRCmd.length() > 1)
          break;
      }
      
    }
    //PINCH
    else if(!strcmp(LRCmd, "L"))
    {
      resetBender();
      resetEmg();
      while(true)
      {
        sampleEMG();
        if( emgAvailable )
        {
           s.incrementT(70, 3);
           s.incrementF1(220, 3);
           s.writeF2(0);
           s.writeF3(0);
           s.writeF4(0);
           resetEmg(); 
        }
        sampleBender();
        if( LRCmd.length() > 1)
          break;
      }
    }
    
    //POINT/COUNT
    else if(!strcmp(LRCmd, "RL"))
    {
      resetBender();
      resetEmg();
      int count=1;
      s.writeAll(toInt(point));
      delay(122);
      while(true)
      {
        sampleEMG();
        if( emgAvailable )
        {
          count++;
           if( count == 2)
             s.writeAll(toInt(count2));
           if( count == 3)
             s.writeAll(toInt(count3));
           if( count == 4)
             s.writeAll(toInt(count4));
           if( count == 5)
           {
             s.writeAll(toInt(relax));
             break;
           }  
           resetEmg(); 
           delay(122);
        }
        sampleBender();
        if( LRCmd.length() > 1)
          break;
      }
    }
    
    //stone
    else if(!strcmp(LRCmd, "RRR"))
    {
      resetBender();
      resetEmg();
      s.writeAll(toInt(grab));
      while(true)
      {
        sampleBender();
        if( LRCmd.length() > 1)
          break;
      }
    }
    
        //scissors
    else if(!strcmp(LRCmd, "RRL"))
    {
      resetBender();
      resetEmg();
      s.writeAll(toInt(count2));
      while(true)
      {
        sampleBender();
        if( LRCmd.length() > 1)
          break;
      }
    }
        
    // Serial.print("Cmd:");
    // Serial.println(LRCmd);
  s.writeAll(toInt(relax));
  resetBender();
  resetEmg();
  beep(600,0);
  }

}

void resetBender()
{
  LRCmd = "";
}

void resetEmg()
{
 emgAvailable = false;  
}


void sampleBender()
{

  if( (millis() - lastLRSampleTime) > benderSampleRate) //delay 100ms before we take next sample
  {
    RbenderVal = analogRead(RPin);   
    LbenderVal = analogRead(LPin);    
    // Serial.println(RbenderVal);
    if(!lastLR)
    {
      if(RbenderVal > RTV)
      {
        lastLR = true; 
        LRCmd +='R';
        beep(300,0);
        Serial.print("Right: "); 
        Serial.println(RbenderVal);
        Serial.println(LRCmd);
      }
      if(LbenderVal > LTV)
      {
        lastLR = true;
        LRCmd +='L';
        beep(300,0);
        Serial.print("Left: "); 
        Serial.println(LbenderVal);
        Serial.println(LRCmd);
      }
    }

    if(  lastLR&& RbenderVal< RTV-15 && LbenderVal < LTV - 15)
    {
      lastLR = false;
      //Serial.println("middle");
    }

    lastLRSampleTime = millis();
  }
}


void printIntArray(int ary[])
{
   for(int i=0; i< sizeof(emgData)/sizeof(int); i++)
  {
    Serial.print( ary[i] );
    Serial.print("  ");
  }
  Serial.println("");
}




uint8_t* toInt ( PROGMEM  prog_uint8_t ary[])
{
  uint8_t ret[] = { int(ary[0]), int(ary[1]),int(ary[2]),int(ary[3]),int(ary[4]) };
 return ret;
}
